// Include files.
#include <exception>
#include "NetworkCommunication/UsageReporting.h"
#include "VersionPrinter.hpp"
#include "Internal/FpkException.hpp"
#include "SystematicRobot.hpp"

using namespace std;

/* The following variable definition is derived from 
 * WPILib, which is licenced under the licence specified
 * in "Licence-WPILib.txt".
 */
/**
 *  The default time per period.
 *  
 *  Will be 0 if it waits until the driver station sends data.
 */
const double SystematicRobot::sDefaultPeriod = 0.0;

/* The following code in this function is derived from 
 * WPILib, which is licenced under the licence specified
 * in "Licence-WPILib.txt".
 */
/**
 *  Constructor for SystematicRobot.
 */
SystematicRobot::SystematicRobot() :
	m_period(sDefaultPeriod),
	m_serviceGame(*this, *m_ds)
{
	m_watchdog.SetEnabled(false);
	SetPeriod(sDefaultPeriod);
	
	AddRootService(m_servicePersistant);
	AddRootService(m_serviceMechanisms);
	AddRootService(m_serviceGame);
	m_serviceRoot.SetRobot(this);
	
	m_serviceRoot.Start();
}

/**
 *  Destructor for SystematicRobot.
 */
SystematicRobot::~SystematicRobot()
{
}

void SystematicRobot::StartCompetition()
{
	Run();
}

/**
 *  Your robot's subsystems and services could be added here.
 *  
 *  Or you could just put it in the constructor.
 *  
 *  Users could override this method to add their robot's subsystems and controllers.
 */
// Define this as an empty function.
void SystematicRobot::Initialize()
{
}

/**
 *  Add another RobotSubsystem.
 *  
 *  @param subsystem The subsystem to add to the robot.
 */
void SystematicRobot::AddSubsystem(RobotSubsystem &subsystem)
{
	m_serviceMechanisms.AddChild(subsystem);
	subsystem.Awake();
}

/**
 *  Add another RobotSubsystem.
 *  
 *  @param subsystem The subsystem to add to the robot.
 */
void SystematicRobot::AddSubsystem(RobotSubsystem *subsystem)
{
	AddSubsystem(*subsystem);
}

/**
 *  Add a Service to be always running.
 *  
 *  @param controller The service to add to the robot.
 */
void SystematicRobot::AddPersistentService(Service &service)
{
	m_servicePersistant.AddService(service);
}

/**
 *  Add a Service to be running when and only when the robot is enabled.
 *  
 *  @param controller The service to add to the robot.
 */
void SystematicRobot::AddEnabledService(Service &service)
{
	m_serviceGame.m_serviceEnabled.AddService(service);
}

/**
 *  Add a Service to be running when and only when the robot is in Autonomous.
 *  
 *  @param controller The service to add to the robot.
 */
void SystematicRobot::AddAutonomousService(Service& service)
{
	m_serviceGame.m_modeAutonomous.GetService().AddService(service);
}

/**
 *  Add a Service to be activate when and only when the robot is in Teleop.
 *  
 *  @param controller The service to add to the robot.
 */
void SystematicRobot::AddTeleopService(Service& service)
{
	m_serviceGame.m_modeTeleop.GetService().AddService(service);
}

/**
 *  Add a Service to be running when and only when the robot is in Test.
 *  
 *  @param controller The service to add to the robot.
 */
void SystematicRobot::AddTestService(Service& service)
{
	m_serviceGame.m_modeTest.GetService().AddService(service);
}

/**
 *  Gets the time between each periodic loop or 0 if it waits until the driver station sends data.
 *  
 *  Almost exactly like IterativeRobot::GetPeriod().
 *  
 *  @returns The time between each periodic loop or 0 if it waits until the driver station sends data.
 */
double SystematicRobot::GetPeriod()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	return m_period;
}

/**
 *  Set the time between each periodic loop or 0 for it to wait until the driver station sends data.
 *  
 *  Almost exactly like IterativeRobot::SetPeriod().
 *  
 *  @param period The time between each periodic loop or 0 for it to wait until the driver station sends data.
 */
void SystematicRobot::SetPeriod(double period)
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (period > 0.0)
	{
		// Not syncing with the DS, so start the timer for the main loop
		m_timer.Reset();
		m_timer.Start();
	}
	else
	{
		// Syncing with the DS, don't need the timer
		m_timer.Stop();
	}
	
	m_period = period;
}

/**
 *  Gets the number of periodic loops per second.
 *  
 *  Almost exactly like IterativeRobot::GetLoopsPerSec().
 *  
 *  @returns The number of periodic loops per second.
 */
double SystematicRobot::GetLoopsPerSec()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (m_period <= 0.0)
		return 50.0;
	return 1.0 / m_period;
}

void SystematicRobot::Run()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	nUsageReporting::report(nUsageReporting::kResourceType_Framework, nUsageReporting::kFramework_Simple);
	
	VersionPrinter::GetEntechFpkVersionPrinter().PrintVersion(4);
	
	Initialize();
	
	m_watchdog.SetExpiration(1);
	m_watchdog.SetEnabled(true);
	
	try
	{
		while (true)
		{
			m_watchdog.Feed();
			
			if (GetIsNextPeriodReady())
			{
				if (IsDisabled())
				{
					//FRC_NetworkCommunication_observeUserProgramDisabled();
				}
				else if (IsAutonomous())
				{
					//FRC_NetworkCommunication_observeUserProgramAutonomous();
				}
				else if (IsOperatorControl())
				{
					//FRC_NetworkCommunication_observeUserProgramTeleop();
				}
				else if (IsTest())
				{
					//FRC_NetworkCommunication_observeUserProgramTest();
				}
				
				m_serviceRoot.RunPeriodic();
			}
			
			m_ds->WaitForData();
		}
	}
	catch (FpkException e)
	{
		printf("Catching Fpk Exception!  %s\n", e.what());
		printf("Throwing back in to the water...\n");
		throw e;
	}
	catch (exception &e)
	{
		printf("Catching Normal Exception!");
		printf(e.what());
		printf("\n");
		printf("Throwing back in to the water...\n");
		throw e;
	}
	catch (exception *e)
	{
		printf("Catching Normal Exception!");
		printf(e->what());
		printf("\n");
		printf("Throwing back in to the water...\n");
		throw e;
	}
	catch (void *e)
	{
		printf("Catching Thrown Object!");
		printf("\n");
		printf("Throwing back in to the water...\n");
		throw e;
	}
}

bool SystematicRobot::GetIsNextPeriodReady()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	if (m_period > 0.0)
		return m_timer.HasPeriodPassed(m_period);
	else
		return m_ds->IsNewControlData();
}

void SystematicRobot::DelayTask()
{
	/* The following code in this block is derived from 
	 * WPILib, which is licenced under the licence specified
	 * in "Licence-WPILib.txt".
	 */
	
	taskDelay(1);
}

void SystematicRobot::AddRootService(Service &service)
{
	m_serviceRoot.AddService(service);
}

Subscriber *SystematicRobot::GetControlLocker(void *obj)
{
	if (m_controlLocks.count(obj) >= 1)
		return m_controlLocks[obj];
	else
		return NULL;
}

void *SystematicRobot::LockControl(Subscriber &subscriber, void *obj)
{
	if (m_controlLocks.count(obj) >= 1)
	{
		/* Something else already has a lock on the object.
		 * It's OK if the original locker is a 
		 * parent of the subscriber/service/operation trying to lock.
		 */
		
		Subscriber *originalLocker = GetControlLocker(obj);
		
		if (originalLocker == subscriber.m_parent)
		{
			// Pass lock from supercontroller to subcontroller.
			m_controlLocks[obj] = &subscriber;
		}
		else
		{
			// Stealing locks is not allowed.  Throw exception.
			throw FpkException("Can not lock an object when already locked.  This can result from starting a service that uses a subsystem already used by another service.");
		}
	}
	
	// Aquire lock.
	m_controlLocks[obj] = &subscriber;
	
	return obj;
}

void SystematicRobot::UnlockControl(Subscriber &subscriber, void *obj)
{
	// Check to see if the controller has a lock on obj.
	if (m_controlLocks[obj] = &subscriber)
	{
		if (subscriber.m_parent == NULL)
		{
			// Subscriber does not have a parent.
			
			// Release lock.
			m_controlLocks.erase(obj);
		}
		else if (subscriber.m_parent->m_controlLocksRequired.count(obj) >= 1)
		{
			/* Controller has a supercontroller and the lock 
			 * originally belonged to supercontroller.
			 */
			
			// Pass lock to supercontroller.
			m_controlLocks[obj] = subscriber.m_parent;
		}
		else
		{
			// Release lock.
			m_controlLocks.erase(obj);
		}
	}
}
